#include "CO_driver.h"


void CO_Node_onlineCheck(CO_Node_t *node);
void CO_Node_feedDog(CO_Node_t *node);


void CO_NMT_Task(Motor_Node_t *co_motor)
{
	uint8_t i;
	
	for(i=0;i<4;i++)
	{
		if(co_motor[i].node_data.node_status != CO_OUTLINE)
			CO_Node_feedDog(&co_motor[i].node_data);
		else
		{
			co_motor[i].motor_data.bootup_status = Bootup_NMT_Setting;
			co_motor[i].motor_data.status = device_no_init;
			co_motor[i].motor_data.motor_config.PDO_config.config_num = 0;
			co_motor[i].motor_data.motor_config.PDO_config.index = 0;
		}
	}
}

void CO_NMT_Txprocess(CO_Node_t *node, OC_MNT_COM_t status)
{
	CO_CANtx_t *tx_packet;
	
	
	tx_packet = osMailAlloc(co_cantx_mailHandle, 1);
	if(tx_packet == NULL)
		return;
	
	tx_packet->id = 0;
	tx_packet->RTR = CAN_RTR_DATA;
	tx_packet->DLC = 2;
	tx_packet->data[1] = node->nodeID;
	switch(status)
	{
		case CO_START_NODE: tx_packet->data[0] = 0x01; break;
		case CO_STOP_NODE: tx_packet->data[0] = 0x02; break;
		case CO_RESET_NODE: tx_packet->data[0] = 0x81; break;
		case CO_PRE_OPERATION: tx_packet->data[0] = 0x80; break;
		case CO_RESET_COMMUNICATION: tx_packet->data[0] = 0x02; break;
		default:break;
	}
	node->online.MNT_waiting = 1;
	osMailPut(co_cantx_mailHandle, tx_packet);
}

void CO_motor_NMTProcess(CO_Node_t *motor, uint8_t *data)
{
	switch(data[0])
	{
		case 0x00:motor->node_status = CO_BOOT_UP;break;
		case 0x04:motor->node_status = CO_STOPPED;break;
		case 0x05:motor->node_status = CO_OPERATIONAL;break;
		case 0x7F:motor->node_status = CO_PRE_OPERATIONAL;break;
		default:break;
	}
	CO_Node_onlineCheck(motor);
	if(data[0] != 0)
	{
		motor->online.MNT_waiting = 0;
	}
}

void CO_NMT_RxProcess(Motor_Node_t *co_motor, uint32_t id, uint8_t *data)
{
	switch(id&0x7f)
	{
		case 1:CO_motor_NMTProcess(&(co_motor[0].node_data), data); break;
		case 2:CO_motor_NMTProcess(&(co_motor[1].node_data), data); break;
		case 3:CO_motor_NMTProcess(&(co_motor[2].node_data), data); break;
		case 4:CO_motor_NMTProcess(&(co_motor[3].node_data), data); break;
		default:break;
	}
}

uint8_t CO_SYNC_start(Motor_Node_t *co_motor)
{
	uint8_t result = 0;
	
	for(uint8_t i=0; i<4; i++)
	{
		if((co_motor[i].node_data.node_status == CO_OPERATIONAL)&&(co_motor[i].motor_data.bootup_status == Bootup_OK))
			result += 1;
	}
	
	if(result>0)
		return 1;
	else
		return 0;
}

void CO_SYNC_Task(Motor_Node_t *co_motor, uint16_t SYNC_cycle)
{
	static uint16_t time_SYNC;
	
	if(CO_SYNC_start(co_motor))
	{
		if(time_SYNC<60000)
			time_SYNC++;
		
		if(time_SYNC>=SYNC_cycle)
		{
			CO_CANtx_t *tx_packet;
			
			tx_packet = osMailAlloc(co_cantx_mailHandle, 1);
			if(tx_packet == NULL)
				return;
			time_SYNC = 0;
			tx_packet->id = 0x80;
			tx_packet->RTR = CAN_RTR_DATA;
			tx_packet->DLC = 0;
			
			osMailPut(co_cantx_mailHandle, tx_packet);
		}
	}
}


void CO_Node_feedDog(CO_Node_t *node)
{
	extern osThreadId OC_txTaskHandle;
	
	if(node->online.time>0)
		node->online.time--; 
	else
	{
		node->node_status = CO_OUTLINE;
		osSignalSet(OC_txTaskHandle, 1);
	}
}

void CO_Node_onlineCheck(CO_Node_t *node)
{
	node->online.time = node->online.outTime;	
}
